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Abstract 

Path following controllers make the output of a control system ap¬ 
proach and traverse a pre-specified path with no apriori time parametriza- 
tion. In this paper we present a method for path following control design 
applicable to framed curves generated by splines in the workspace of kine¬ 
matically redundant mechanical systems. The class of admissible paths 
includes self-intersecting curves. Kinematic redundancies are resolved by 
designing controllers that solve a suitably defined constrained quadratic 
optimization problem. By employing partial feedback linearization, the 
proposed path following controllers have a clear physical meaning. The 
approach is experimentally verified on a 4-degree-of-freedom (4-DOF) ma¬ 
nipulator with a combination of revolute and linear actuated links and 
significant model uncertainty. 


1 Introduction 

The problem of designing feedback control laws that make the output of a con¬ 
trol system follow a desired curve in its workspace can be broadly classified as 
either a trajectory tracking or path following problem. As previously suggested 
in [1], trajectory tracking consists of tracking a curve with an assigned time pa¬ 
rameterization. In other words, besides the shape of the curve, the end-effector’s 
motion has a desired velocity and acceleration profile. This approach may not be 
suitable for certain applications as it may limit the attainable performance, be 
unnecessarily demanding or even infeasible. For example, the primary objective 
of a robotic manipulator tracing a contour in its output space in welding [2] or 
cutting [3] applications is to accurately traverse a path. The trajectory tracking 
approach parametrizes the desired motion along the path and a controller is de¬ 
signed to asymptotically stabilize the tracking error to zero. When the tracking 
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error is non-zero, the controller may drive the end-effector to “cut through” an 
obstacle in order to reduce the tracking error and thereby not precisely follow 
the contour. 

On the other hand, path following or manoeuvre regulation controllers, as 
previously defined in [1], drive a system’s output to a desired path and make the 
output traverse the path without a pre-specified time parametrization. Thus, 
a path following controller stabilizes all trajectories that cause the system’s 
output to traverse the desired path, whereas trajectory tracking controllers do 
this for a single trajectory. In general, path following results in smoother con¬ 
vergence to the desired path compared to trajectory tracking, and the control 
signals are less likely to saturate [4]. It is also shown in [5] for the linear case, 
and [6] for the nonlinear case, that for non-minimum phase systems, path follow¬ 
ing controllers remove performance limitations compared to trajectory tracking 
controllers, which have a lower bound on the achievable /l 2 -norm of the tracking 
error. 

There are several approaches for implementing path following. The papers 
[1, 7, 8] project the current system state onto the desired path in real time. 
Then a tracking controller is used to stabilize this generated reference point. 
Contour error mitigation controllers have also been proposed in the machining 
community in addition to a trajectory tracking controller in order to drive the 
machine tool towards a path [9, 10, 11]. However, these approaches do not 
guarantee that the desired path will be invariant, i.e., if for some time the 
output is on the path and has a velocity tangent to the path, then the output 
will remain on the path for all future time. 

Virtual holonomic constraints can be used to guarantee invariance of a path 
via set stabilization. This methodology is used for orbital stabilization for a 
Furuta pendulum [12], helicopters [13], and, in the sense of hybrid systems, for 
the walking/running of bipedal robots [14]. Also, [15] introduces conditions for 
feedback linearization of the transverse dynamics associated with periodic orbits. 
This inspired the work in [16] in which an arbitrary non-periodic path in the 
output space can be made attractive and invariant using transverse feedback lin¬ 
earization (TFL) for single-input systems. This idea was further extended in [17] 
by providing conditions for which desired motions can be achieved while on the 
path, as opposed to orbital stabilization where the dynamics along the closed 
curve in the state space are fixed. The approach in [17] simplifies controller 
design and decomposes the path following design problem into two independent 
sub-problems: staying on the path and moving along the path. 

However, in [17], perfect knowledge of the dynamic parameters of the sys¬ 
tem was assumed, performance was demonstrated only for a linear, decoupled 
system, and the path is restricted to a special case of simple curves such as 
circles. One contribution of this paper is that we enlarge the class of allowable 
paths, allowing self-intersecting paths and curves that are generated through 
spline interpolation of given waypoints. This is accomplished by designing a 
numerical algorithm and by using a systematic approach to generate zero level 
set representations of the curves. Another contribution of this work is extending 
the class of systems of [17] to mechanical systems with kinematic redundancies. 
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by employing a novel redundancy resolution technique at the dynamic level, 
yielding bounded internal dynamics. Khatib proposed a similar redundancy 
resolution mechanism for a specific class of mechanical systems in [18], namely 
the torque-input model of robot manipulators. However the approach in this 
paper is to resolve the redundancies directly in the context of path following via 
feedback linearization, unlike in [18], and is thus applicable to general mechan¬ 
ical systems, which may include a dynamic map between the system input and 
the output forces. To the best of the author’s knowledge, redundancy resolution 
in the context of feedback linearization is an unsolved problem. We show that, 
under the assumption that the redundant system has viscous friction on each 
degree of freedom, the control law obtained by solving a static, constrained, 
quadratic optimization problem yields bounded closed-loop internal dynamics. 
We further validate the control strategy on a non-trivial redundant platform 
with significant modelling uncertainties using a robust controller. 

In this paper, the class of paths that can be followed are broadened, made 
possible by systematic approach to generating zero level set of a function, and 
using a numerical algorithm in the controller. A novel approach to resolving re¬ 
dundancies is proposed. Finally, non-trivial experimental verification including 
robustihcation is performed. 

The detailed problem statement and proposed approach are outlined in Sec¬ 
tion 2. The control design is explained in Section 3 and experimental results 
are shown in Section 4. 

1.1 Notation 

Let (a:, y) denote the standard inner product of vectors x and y in R". The 
Euclidean norm of a vector and induced matrix norm are both denoted by H-H. 
The notation s o h : A ^ C represents the composition of maps s : B C 
and h : A ^ B. The *th element of a vector is denoted [x]i. The symbol := 
means equal by definition. Given a mapping </> : R" —^ R™ let d(j)x be its 
Jacobian evaluated at x G R". If /, g : R" —>■ R" are smooth vector fields 
we use the following standard notation for iterated Lie derivatives Vjcj) := (j), 
L’jcf) := Lf{LY^(j)) = (dL^"Va:,/(a;)), Lghfcj) := Lg{Lf(j)) = {dLf(j)x,g{x)). 

2 Problem Formulation 

We consider path following problems in the workspace of kinematically redun¬ 
dant, fully actuated, Euler-Lagrange systems. In this section we define the class 
of systems considered, the class of allowable paths and conclude by stating the 
problem studied. 

2.1 Class of systems 

We consider a fully actuated Euler-Lagrange system with an Wdimensional 
configuration space contained in R^ and N control inputs u G R'^. The model 
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is given by 


^dC _ (K 

dt dq dq 


A{q)u 


where g, g G are the configuration positions and velocities, respectively, of 
the system, and £(g, q) is the Lagrangian function. We assume that C is smooth 
and has the form C{q,q) = K{q,q) — P{q) where K{q,q) = {l/2)q^D{q)q is the 
system’s kinetic energy and P : —>■ M is the system’s potential energy. The 

inertia matrix D{q) is positive definite for all q. Furthermore, A : —>■ , 

the input matrix (usually identity), is smooth and nonsingular for all q. 

The system can be rewritten in the standard vector form 


D{q)q + C{q,q)q + G{q) = A{q)u (1) 

where C{q, q) G represents the centripetal and Coriolis terms, and G{q) = 

(dPg)^ = VP(g) G represents the gravitation effects [19]. Defining Xc ■= q, 
Xv := q, n := 2N, and x '■= (xc,a;«) G x we can express (1) in state 
model form 


i = f{x)+ 9 {x)u := 


fv{x) 


0 

t \ u, 

9v{Xc)j 


( 2 ) 


where / : K” —)■ K" and 9 : K” —>■ are smooth, 9v{xc) '■= D ^{xc)A{xc) 

and fy(x) := -D-^{xc) (G(x)xy + G{xc)). 

Motivated by the task space of a robotic manipulator, we take the output 
of system (2) to satisfy 


y = h{x), =0, y (3) 

where p is the dimension of the output space and h : K" —>■ is smooth. Let 
J(xc) := the output Jacobian, be non-singular. We allow the system to be 
redundant in the following sense. 


Definition 2.1. System (2), (3) is redundant if > p. 


2.2 Admissible paths 

We assume that we are given a finite number, rispiine G N, of (p -I- l)-times 
continuously differentiable, parametrized curves in the output space of (2) 

(Tfc : M —>■ K^, k G {1, 2,..., rispiine} • (4) 

The curves need not be polynomials, but we nevertheless use the term spline in 
accordance with common practice. We are also given rispUne non-empty, closed, 
intervals of the real line Ik ■= [A(fc,min)) A(fc,max)] C K. The desired path is the 
set 

"^spline 

P:= U fTfc(Ifc). (5) 

fe=i 

We assume that the overall path is (p -I- l)-times continuously differentiable in 
the following sense: 
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Assumption 1 (smoothness of path). The curves (4) satisfy 


(Vfc e {1,2,... ,nspime - 1}) (Vi e {0,1,... ,p + 1}) 


dVfc 


dA* 


dVfc+i 


A=A 


{fe,l 


dA* 


A=A 


(<=+ 1,1 


In the case of closed paths, the same equality must hold between spline index 1 
and T^spline ■ 

This assumption is required to ensure continuous control effort over the 
entire path. In the case of spline interpolation (fitting polynomials between 
desired waypoints). Assumption 1 can be enforced by setting it as a constraint 
in the spline fitting formulation and using polynomials of sufficiently high degree. 
Typically the parameter A of each curve ct/c represents the chord length between 
waypoints; namely, A(fe_min) = 0 and A(fc^i„ax) = h where Ik is the Euclidean 
distance between the fc’th and {k + 1) waypoints [20]. 

Remark 2.2. Assumption 1 can be relaxed if continuity of the control signal 
over the entire path isn’t required. Furthermore, if Assumption 1 is only true for 
i = 0, i.e., the path is not continuously differentiable, the control input can still 
be kept continuous by designing the desired tangential velocity profile in such a 
way that the output slows down and stops at o'k{X(^k,max)) = c’'fc+i(='^(fc+i,min ))5 
and then resumes motion along curve A: + 1 in finite time. See Remark 3.4 • 

We further assume that the curves (4) are framed. 

Assumption 2 (framed curves). For each spline k, the first p derivatives of ak 
are linearly independent, i.e., 

{Mk e {1,2, ...,nspii„e})(VA G Ife) span^ |cr{,( A), c7{'( A),..., 4^^ (A) | = 

Assumption 2 allows the use of Gramm-Schmidt orthogonalization to con¬ 
struct Frenet-Serret frames (FSFs) in the output space The use of FSFs 
for control of mobile robots is a well-known technique [21]. In this paper we 
use Assumption 2 to generate a zero-level set representation of V in the state 
space of (2). Previous works [17] relied on the zero-level set representation to al¬ 
ready be available by restricting the class of paths to one-dimensional embedded 
submanifolds, which precludes self-intersecting curves. 

Remark 2.3. Assumption 2 can be relaxed to only requiring that the curve 
be regular by not using FSF to construct a basis that aligns with the curve at 
each point along the curve. Consider a straight line o'(A) = [A O] G K^. This 
curve violates Assumption 2 at all points A. The first basis vector is taken to be 
the unit tangent vector [l O] (as is the case in the FSF). The normal vector 
cannot be constructed via FSF, since the curvature is zero. However, one can 
choose the normal vector to be [O l] . 

Furthermore, consider a regular torsion free curve in the a;j/-plane of a 3- 
dimensional workspace. This curve also violates Assumption 2, since cr^^^(A) 
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will be a linear combination of a' and a”. However, one can choose the first 
basis vector to again be the unit tangent vector . The unit normal vector 

can be chosen to be the 90 degree rotation of the first basis vector, and the 
final basis vector can be chosen to be [O 0 l] . Once the appropriate frame 
is chosen, the proposed approach for coordinate transformation and control 
design (Section 3) can be applied without any additional modifications. • 

Path following control design can naturally be cast as a set stabilization 
problem. This point of view was successfully taken in [22, 17]. We also take this 
approach but, unlike previous work, we do not require that the path V be an 
embedded, nor immersed, submanifold of the output space. In other words the 
curve may be self-intersecting. Removing this restriction is particularly useful 
because it allows the curve to go through an arbitrary set of waypoints in the 
output space of system (2) and can be constructed using classical spline fitting. 

2.3 Problem statement 

The problem studied in this paper is to find a continuous feedback controller for 
system (2), with ngpUne curves that satisfy Assumptions 1 and 2, that renders the 
path V in (5) invariant and attractive. Invariance is equivalent to saying that if 
for some time t = 0 the state a:(0) is appropriately initialized with the output y = 
h{x{0)) on the path V, then (Vt > 0) h{x{t)) e V. Attractiveness is equivalent 
to ensuring that for initial conditions a;(0) such that the output /i(x(0)) is in a 
neighbourhood of the desired path V, h{x{t)) under (2) approaches V. 

The motion along the path should also be controllable, so that a tangential 
position or velocity profile can be tracked. Furthermore, any remaining 

redundant dynamics (will show up as internal dynamics for the closed-loop 
system) must be bounded. 

3 Multiple Spline Path Following Design 

We begin by defining a coordinate transformation in Section 3.1 that depends on 
a given path segment (4) and the system dynamics (2) . These coordinates parti¬ 
tion the system dynamics into a sub-system that governs the motion transversal 
to the desired path and a tangential sub-system that governs the motion of the 
system along the path (Section 3.2). The tangential dynamics are further de¬ 
composed to reveal the redundant dynamics. It will be shown that the system 
in transformed coordinates has a structure that facilities control design, namely 
the dynamics will appear linear and decoupled. One of the keys to this construc¬ 
tion is the ability to unambiguously determine the spline that is currently closest 
to the system output (3) . By doing so, we are able to deal with self-intersecting 
curves and curves (5) that aren’t embedded submanifolds. A novel redundancy 
resolution approach is then proposed to ensure the redundant dynamics remain 
bounded (Section 3.3). The overall block diagram can be found below in Figure 
1 . 
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a’fc(A), k € {1,... 

7 ^spline} 


Figure 1: Entire control system block diagram. 


3.1 Coordinate Transformation 

We construct a coordinate transformation T that maps the state x of system (2) 
to a new set of coordinates that correspond to the tangential position/velocity 
along the path, and the distance and velocity towards/transversal to the path. 

3.1.1 Tangential States 

For the moment assume that the closest spline to the output y is known; denote 
it as k*. In the case that rispiine = 1, the k notation can be dropped to ease 
readability. Furthermore, denote the parameter of spline k* that corresponds 
to the closest point to the output y as X* G 1^* [17] 

A* :=wk*{y) :=arginf |l 2 /-(Tfc.(A)|| . (6) 

AGlfc. 

The first tangential state is the projected, traversed arclength along the path: 

A:*-l 

m = (x) := Sk^ o Wk^ o h{x) + ^ Sk{X{k,max)) (7) 

k^l 

where 

A 

Sfc(A) := J 

This integral does not have to be computed for real time implementation if only 
tangential velocity control is required, which will be the case in the experiment 
(Section 4). Note the slight abuse of notation for using iji to represent both 
the state and the map. The summation term on the right in (7) is used to get 
the arclength over the entire path V, so that rji remains continuous even as the 


dcrfc.(A) 

dA 


dA. 
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current spline number k* increments/decrements. Note that this summation can 
be computed offline and accessed as a look-up-table during real-time control. 

With rji representing the projected tangential position of the output y along 
the path, tangential velocity is computed by taking the time derivative. By the 
chain rule, 


m = ■= m = 


dsfc* dzuk* dh 
dA* dy dxc 




( 8 ) 


(see Appendix B for complete derivation) where ei is the unit-tangent Frenet- 
Serret vector: 




dgfc* 

dsfc. 


dcTfc* dA* _ ct(,.(A*) 
dA a=a* dsfe- lkfc.(A*)ir 


(9) 


3.1.2 Numerical Optimization for A* and k* 

The tangential coordinates defined by (7), (8) rely on the computation of the 
parameters k* - which characterizes which of the UspUne curves crfc(Ifc) is closest 
to the system’s output - and A* - which characterizes the point fTfe*(A*) closest 
to the system’s output. 

Assumption 3 (Initial A* and k* are known). Based on the initial conditions 
of the system at time t = d, the corresponding k* G {1, ...jnspiine} and A* G 
are known. 

Assumption 3 can be satisfied by apriori global minimum brute-force opti¬ 
mization solvers to find the closest point of h{x{0)) to the path V. One can 
do this by quantizing the path V and searching for the point with minimum 
distance to the output, whilst keeping track of the curve number k and the 
path parameter A. The output to this approach will not yield the exact solu¬ 
tion (due to the quantization of 7^), but will be adequate for sufficiently small 
quantization. In the case that multiple global minima exist, one can be chosen 
arbitrarily. Then for t > 0, a numerical local optimizer like gradient descent 
can be used online, initialized at the previous time step’s k*, X*. The algorithm 
we developed (Algorithm 1) can be found in Appendix A. 

Remark 3.1. In [17], the path to be followed was assumed to be a one¬ 
dimensional embedded submanifold, which precludes self-intersecting curves. 
By employing Algorithm 1 under Assumption 3, this work removes that restric¬ 
tion and enlarges the class of admissible paths. 

Consider first the case in which two curves aj, i,j G {1,. •., ngpiine} 
overlap. That is, there exist two closed, possibly disconnected, sets A^ C li, 
Aj c Ij with (VAi G Ai)(3Aj G Aj) ai{Xi) = aj{Xj). Suppose that at the 
current time-step, we have k* = i and A* = Xi G A^. Then the coordinate 
transformation employed is completely determined by the curve Ui. At the 








next time-step, the algorithm will search for its next update. This local 
search implies that the algorithm will not “jump” to Ij despite the existence of 
intersection points. 

The second case, the one in which there is single curve that is not injective, 
is handled similarly. Suppose there exist two values Ai, A 2 S Ife*, Ai < A 2 , with 
o'fe*(Ai) = CTfe*(A 2 ). By continuity, it is possible to find two open intervals Ii, 
I 2 C with Ai S Ii and A 2 G I 2 and Ii n I 2 = 0. Now suppose that at the 
current time-step, we have A* = Ai. At the next time-step, the algorithm will 
search Ii for its next update. This local search implies that the algorithm will 
not “jump” to I 2 despite the existence of an intersection point. 

In addition, when the output y is equidistant to multiple points on the path, 
the algorithm will again just choose the A* that is closest to the previous time- 
step’s A*, due to the local nature of the monotonic gradient descent algorithm. 
• 


3.1.3 Transversal States 


The transversal states represent the cross track error to the path using the 
remaining FSF (normal) vectors as follows. The fc* notation will be dropped for 
brevity. The generalized Frenet-Serret (FS) vectors are constructed applying 
the Gram-Schmidt Orthonormalization process to the vectors cr'(A), cr"(A), ..., 
a(P)(A): 


ej(A) 


ej(A) 

l|e,(A)|| 


( 10 ) 


where 


t-i 


e,(A) := cr('^^(A) - ^ (A), e,(A)^ ei(A), 


i=l 


for j G {1,... ,p\. This formulation is well-defined by Assumption 2. For curves 
that violate Assumption 2, other ad-hoc orthonormalization processes can be 
used, see Remark 2.3. The transversal positions can be computed by projecting 
the error to the path, y — (j(A*), onto each of the FS normal vectors: 

:= (e,(A*),M^) - (11) 


for j G {2,... ,p}. Taking time derivatives yields (see Appendix B for complete 
derivation) 


^2 '-^2 \ x )= i { ' = II jff )|| - g ( A *)) 

(A \ \x^—'Djoh{x)i (1^) 

j G {2, ...,p}, and e'(A*) are given in (A. 2). If p = 2, then e 2 (A*) and 
h{x) — cr(A*) are orthogonal and ^2 simplifies to (e 2 (s(A*)), J(xc)xy). Figure 2 
illustrates the p = 3 case. 
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3.1.4 Diffeomorphism 

In this section, we show that the tangential and transversal maps defined above 
can be used to construct a diffeomorphism. For brevity we again drop the spline 
k notation. First, define the lift of curve a to K" as 

F := {x e M" : = ... = = 0} 

which represents the states x that correspond to the output y being on the curve. 
The above set is not necessarily a manifold since the curves considered can be 
self-intersecting or even self-overlapping, thus is more general to the restricted 
manifold case analysed in [17]. 

Lemma 3.2. There exists a domain U C K” with T G U such that for all 
X G U, and (6) is solved by Algorithm 1, the 2p functions (7), (8), (11), (12) 
have linearly independent differentials. 

Proof. See Appendix B. □ 

By [23, Proposition 5.1.2], for each x* S U, it is possible to find a function 
(fi : K" —> so that by the Inverse Function Theorem [23] there exists a 

neighbourhood Ux* such that the mapping 

T :Ux* ^ T{Ux-) C K” 

(?7i,»72,?i\c2\---.cr\cr\c) 

= (m {x ), r ]2 (x) ,fl{x),fl{x),..., 

ei-\x),e2-\x),p{x)) 
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is a diffeomorphism onto its image. In practice the domain of T can often be 
extended to all of U (see Section 4). 

Let us bring back the spline notation k to explicitly indicate that the coor¬ 
dinate transformation is dependent on the curve fc*, which is determined using 
Algorithm 1. Namely, the coordinate transformation can be viewed as a collec¬ 
tion of rispiine maps 

Tfe : C/fc C M" ^ Tfc(C/fc) C M" 

a; ^ ivu {x),V 2 ^ (a;), {x},..., 

ei;\x),e2:\x),M^)) = 

Sfc(A*) -I- X]fe=l Sk{^(k,max)) 

{ei^{X*),J{xc)xy) 

{e 2 ^{X*),h{x) - crfc(A*)) 

||^?({!)|| (e 2 ,,(A*),/i(x) - crfc(A*)) -f (e 2 ,(A*), J(xc)x„) 

{ep^iX*),h{x) - (Tk{X*)) 

{e'p^iX*),h{x) - ak{X*)) + {ep,{X*),J{xM 

Vk{x) 

where A* = Wk o h{x), k G {1,..., u.spii„e}. As we show later, satisfying a path 
following task depends on a state feedback involving the r] and ^ states. Thus, 
it is desirable for rj and ^ to remain continuous from one curve on the path to 
the next, i.e., as k changes via Algorithm 1, to avoid wear on equipment due to 
jerky motions. The following proposition will show this. 

Proposition 3.3. The coordinate transformation 

Tk{x) := [??i,(x),ry 2 ,(x),Ci\(x),^ 2 \(a;),--,Ci,"^(^)’C”^( 2 :)]’^ (14) 

is continuous as k changes by Algorithm 1. 

Proof. Based on Algorithm 1, k changes when the output of the system crosses 
the hyperplane shown in Figure 3. Thus we must show that Tk{x) = Tk+i(x) for 
X that correspond to the output y = h{x) on this hyperplane. This is equivalent 
to showing 

(Vx G{xG K”|x = {wk O h(x))“^(A(fc^max))}) Tk{x) = fk+l{x). 

First, the coordinate transformation relies on the computation of A* = Wkoh{x), 
which can be easily verified to be continuous in x for a fixed k. The tangential 
position for a fixed k is continuous since Sfc(A*) is a function of (t'(A*), which 
is continuous by Assumption 1. On the hyperplane, r]i^(x) = Sk{X(^k,max)) + 

Si(X(k,max)) = Tii=i Si(X(k,max)) = (x) and is Still continuous at the 

transition. 
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Figure 3: Spline k and spline fc + 1, their stitch point a-k(X{k,max)) = 
crfe+i(A(fc+i_min)), and the hyperplane. 


For the remaining states, h{x), J{x) are continuous by the class of systems 
(2). The remaining terms are the FS vectors and its derivatives. For a fixed 
k, the FS vectors ej^(A*) depends on for j = by (10), and 

thus will be continuous by Assumption 1. Furthermore, the derivative of the 
FS vectors are governed by the FS equations for general spaces (A.2) , and 

depend only on the generalized curvatures A)(A) = , i G {1, — 

1}. Thus, the highest derivative that appears again is and so the FS 

vector derivatives will remain continuous for a fixed spline k. At the transition 
point, ej^{X(^k,max)) = ej^^^{X(^k+i,min)) holds if and only if crl,^\X(^k,max)) = 

o'fc+i(A(fc+i,niin)) for j G {1, ... ,p}. This is satisfied by Assumption 1. To show 
that e'^(A(fc,„iax)) = (A(fe+i,min)) for j G {l,...,p}, by (A.2) it suffices 
to show that the generalized curvatures are continuous at the transition, i.e., 
^tki\k,max)) = Xik+ii\k+i,mm)) for * € By definition of the 

curvatures, this then simplifies to showing e'j^{X(^k,max)) = e.'j^^^{X{k+i,rain)) for 
j € {1, ...,p—l}. These derivatives can be expressed by evaluating the derivative 
of (10), and so the equation will hold if <Jk\^{k,max)) = CTfc+i(A(fc+i,min)) for 
j G {1,... ,p}. Again by Assumption 1 this holds. □ 
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3.2 Dynamics and Control 

The dynamics in transformed coordinates are 


m = m 

f]2 = L'jrjiix) + LgLf'qi(x)u 

4' = 

il = L)i\{x) + LgLf£,l{x)u 

. (15) 

CP-l _ CP-1 
SI — S2 

ir^ = L)e,-\x)+LgLfe,-\x)u 
C = Lfip[x) + Lg(p{x)u =: f^ix, u) 


where x = T The Lie derivatives can be computed given the state 

maps from Section 3.1.4 and the class of system (2): 


L}m{x) 


LgLfT]i{x) 


+e'(A*) 

LgLf^i-\x) 


~ ||cr'(A*)|| ),J{Xc)Xv) 

= ei{X*y J{xc)gv{xc) 

= (^ej{X*), J[xc)fv{x'^ 

+ (e'(A*),2J(xc)x„ -ei?72) 

/ LgLfr]i{x) _ 2 ((7'(A*), g"(A*)) \ \ 

V lk'(^*)ll " lk'(A*)f ) j 

= ej{X*)^ J{xc)gv{xc) 


+ {h{x) 


a(A*))^e;(A*) 


LgLfgi{x) 

lk'(A*)|| 


for j e {2,... ,p} and X* = w o h{x). Physically, see their dehnitions in Section 
3.1, the ry-subsystem represents the dynamics tangent to the path, and the C 
subsystem represents the dynamics transversal to the path. Thus, the redundant 
dynamics represent the dynamics of the system (2) that do not produce any 
output motion. 
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It then follows from Lemma 3.2 that the decoupling matrix 


I3{x) := 


LgLf mix) 
LgLf^lix) 


[LgLfCr\x)\ 


lk'(A*)ll 


ei(A*)^ 

+ G2(A*)^ 


(h{x)-a{\'’))^ f 
L ||<t'(A*)|| 


{X*)e,{X*y + ep{X*y 


J {,Xc)Qx (Xc) 


has full rank for all x G U. This is equivalent to saying that for each x G U, 
system (2) with output has a vector relative degree 

of {2,..., 2} and we can perform partial feedback linearization. Let 


a{x) 


L)m{x) Lj^Hx) 



(16) 


and introduce an auxiliary input v = [vrj G K x such that 

V = f3{x)u + a{x) (17) 

the dynamics (15) become 

C = fc{x,u) ^ 1=^2 ?i =^2 

m =Vr, ii= V^j 

where j G {1,... ,p — 1} and x = T~^(r], C). 

In U the ^-subsystem is linear and controllable, and can be stabilized to 
ensure attractiveness and invariance of the path V. One can do this using a 
simple PD controller v^j = —Kp^{ — j G {l,...,p— 1}, for positive 

Kp, Kp. Another transversal controller is (22) which is used on the experimen¬ 
tal platform to overcome modelling uncertainties. Doing so will stabilize the 
largest controlled invariant subset of L known as the path following manifold 
[17]. The ? 7 -subsystem is also linear and controllable, thus a tangential controller 
can be designed for Vri to track some desired tangential position or velocity pro¬ 
file A block diagram of the entire control system can be seen in Figure 

1 . 


Remark 3.4. Suppose a desired tangential velocity trajectory is to be tracked, 
772 ®^(t). In the case that Assumption 1 fails for i > 0, 772 ®^(t) can be designed 
such that it approaches zero at the junction of the splines, then ramping up to, 
for instance, some desired steady state value just after the junction. This way, 
the control input remains continuous and the system avoids unwanted transients 
associated with the differential discontinuities of the path. • 
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3.3 Redundancy Resolution 

If n = 2p, there is no ip map and thus no redundant dynamics. However, if 
n > 2p, then the system has internal dynamics. Unlike kinematic redundancy 
resolution techniques [19], we seek a scheme at the dynamic level that can 
incorporate actuator constraints while ensuring the internal dynamics of the 
system are bounded. The approach should also work when the joint-space and 
task-space dynamics are not necessarily related by a static input conversion 
map (as is the case for torque-input model of a robot manipulator [18]). Such 
systems include a combined motor and manipulator model (see Section 4, where 
a dynamic relation exists between motor input voltage and end-effector force), 
or mobile manipulator systems. 

Suppose the ^-subsystem is stabilized and the ry-subsystem is tracking a 
desired profile 7?“^®^(t) by some outer loop controller for v. The zero-dynamics 
correspond to the redundant dynamics ( = f(^{x,u) under the transform (17), 
X = 0, 0 and Vrj = = 0. In order to ensure a well-behaved 

response, we must ensure boundedness of the zero dynamics. 

With V generated by some outer loop controller, there is some freedom in the 
choice of the control input u under the feedback transform (17) (recall v G K^, 
u € and n > 2p); this freedom can be used to enforce boundedness of 

the zero dynamics. Since the relation from u to the states is dynamic, one can 
use dynamic programming to achieve some desired (and bounded) trajectory in 
the zero dynamics. However, this in general is quite complicated. This is why 
previous works on path following have relied on the zero dynamics being absent 
or assumed to be stable. To make things simple, consider the static optimization 

mm(u — r(x))^ W (u — r(x)) (18) 

U 

under the constraint (17). It is a static minimization of a quadratic function of u 
under a linear constraint, or which a closed form solution for u can be solved for 
using Lagrange Multipliers. The matrix W G is an invertible weighting 

matrix, and the function r : K" —>■ K" is used to bias the control input u to 
achieve desired behaviour in the zero dynamics. For example, if r{x) = 0, then 
we are minimizing control effort while staying on the path and following some 
desired ry''®^(t). 

Most real systems have actuation and configuration limits. Thus, another 
useful criteria for redundancy resolution is to design u so that the configuration 
variables Xc are driven away from their limits. For example, consider a joint on 
a manipulator. If this joint is at its minimum value, setting the control effort 
corresponding to this joint to be the maximum control effort will likely increase 
the value of the joint, thereby pushing it away from the negative joint limit. 
The corresponding r function to achieve this behaviour in (18) is 

r{xc)i = - {xci - Xci . ) + (19) 

for z S {1,..., iV}, or graphically shown below in Figure 4. 
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r{xc)i 



Figure 4: The function r{xc) for avoiding joint and actuation limits. 


Using Lagrangian Multipliers, the solution to (18) under constraint (17) is 
u = ^\x){v - a{x)) + {Inxn “ ^'^{x)l3{x)) r{x) ( 20 ) 

where /3'l’(a;) = W~^j3(xY {l3{x)W~^fi{x)^) ^ and Inxn is the identity matrix. 

Conjecture 3.5. Using (20) and (19) to generate the control input u results 
in boundedness of the redundant dynamics ^ = f(^{x,u), while achieving path 
following and maintaining joint limits, in the situation that system (1) has in¬ 
herent damping. 

Conjecture 3.5 has been tested and verified on the following two examples 
1-2, and on our experimental platform in Section 4. 

Example 1. (2-DOF linear system) Consider the linear system in Figure 5 
with output y € M. taken to be the position of the top-most block. Let mi, bi 
and Ui be the mass, coefficient of friction, and force acting on each respective 
block. The corresponding dynamics are 

Figure 5: A Planar 2-DOF fully-actuated system where toi, m 2 , 6 i, 62 > 0. 
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For this particular example, N = 2,p = 1. Thus p — 1 = 0 and there are no 
transversal dynamics. Therefore, under our path following context, the position 
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of the output along the path is 771 = X 3 . The remaining function —>■ 

is chosen to complete a diffeomorphism. For instance 

T : ^ 

X !-)■ {r]i{x),r]2{x),Cl{x),C2{x)) = {X3,X4„Xi,X2) 
yields the dynamics in the transformed coordinates 


m = m 

^2 

V2 = - 


m 2 


(C 2 


Cl = C2 


172) H- U2 =: Vrf 

m 2 


C 2 


f h+b2 

V 


C2 + 


b 2 

mi 


m + 


1 

-Ml. 

rui 


The corresponding terms in the feedback transform (17) are a{x) = ^{x 2 — X 4 ) 
and I5{x) = [O . Now assume the desired tangential reference trajectory is 



for some constant 77 ^®^ The control law — i?i) + Kd{—t] 2 ) for 

positive KpjKp) will force rj to converge to 77 ™^. Thus the zero dynamics are 
the redundant dynamics when 77 = 77 "'®^ and a{x) + I3{x)u = Vj^ = 0, i.e. 


Cl 

C 2 



C2 H- Ml- 

mi 


Using the controller (20) yields the zero dynamics 


C2 


C2 


mi 


C2 + — r{x)i. 

mi 


and thus the zero dynamics can be made globally exponentially stable if, for 
example, r(x)i is a linear function of Ci = xi with negative slope. This is 
precisely the case with our proposed r function (19). Furthermore, the equi¬ 
librium point will be C 2 = 0 , and the value of x = ,(^ 1 X 2 = 0 ) for 

which r(x)i vanishes. In our proposed r function, this occurs when (ji = xi = 
^i^ax ^ (xi^„^ - Xi„,„) -f Xi„,„. If Mi^^^ = -Mi„,„, which usually is the 


case for practical examples, then the equilibrium point is Ci = +^ima^)) 

the average value of the joint limits. Thus, the path following constraints are 
met and the zero dynamics are stabilized while achieving the objective of staying 
within joint limits using (19). A 
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Figure 6: A Planar 3-DOF fully-actuated manipulator whose task is to stabilize 
a point on a circle. 


Example 2. (3-DOF planar manipulator) Consider a planar 3-DOF robot 
manipulator and the desired path of a circle, and the point ryi = 0 on the path 
is to be stabilized, shown in Figure 6. 

Here N = 3,p = 2 and thus the dimension of redundancy is 1. Let’s further 
assume that each link has unit mass and inertia, with the center of mass of 
each link be at the midpoint of each link. Without loss of generality, we assume 
gravity is ignored. The dynamics can be derived as per [19] to yield the form 
( 1 ). 


Let the redundant state be Ci = Ci = Xci + Xc^ + Xc^ , the sum of the 
joint angles, the angle the end-effector makes with the ground. Thus C ,2 = 

Xvi +Xy^ +Xy^, and C 2 = Ifi^) + ■ The zero dynamics 

correspond to these redundant dynamics when ^ = 0,?7 = 0 and thus u = 0 . 
Stability of the zero dynamics depends on the choice of u. 

Suppose that u is chosen as in (20) with r from (19). The parameters for the 
r function are = 10 for i = 1,2,3. The joint limits are chosen 

so that X = T~^{r] = 77 ’'®^, (ji = 0, ^2 = 0) is the average value of the joint limits. 
This is done to see if C = (0,0) will become an equilibrium point for the zero 
dynamics. 

The resulting phase curves for the zero dynamics for the 3-DOF simple robot 
case with viscous friction can be found in Figure 7. 

Notice there are two equilibrium points in the zero dynamics phase plane: 
One is unstable and one that is asymptotically stable. The unstable equilibrium 
point is the singularity configuration of the manipulator on the zero dynamics 
manifold. The asymptotically stable equilibrium point is located at C = (0,0), 
which corresponds to the joint positions being at the midpoint of their joint 


18 







3 


^ Cl [rad] 

Figure 7: Phase portrait of the zero dynamics for the 3-DOF manipulator ex¬ 
ample with 0 = 0. 


limits. Furthermore, if the system were to have no viscous friction, C = (OjO) 
will no longer be asymptotically stable. A 

Remark 3.6. The proposed approach assumes that the system is fully actuated. 
To examine the under-actuated system case, let m be the number of inputs, p 
be the dimension of the output space, and N be the degrees of freedom of the 
system. There is a basic requirement ([17]) that to > p — 1 in order to only 
follow the path, and to > p in order to follow the path and move along it in a 
desired fashion. 

If TV > TO = p, it is still possible to follow the path in a desired fashion, 
however the internal dynamics would be completey uncontrollable, since all the 
freedom over the control input will be used to stay on the path and traverse the 
path. If = TO > p, then Conjecture 3.5 applies. If iV > to > p, then there will 
be some uncontrolled internal dynamics and the proposed redundancy resolution 
approach may not work to ensure boundedness of these internal dynamics. • 

Remark 3.7. In the case that the system hits a singularity {J{x) loses rank), 
the controller will fail as the decoupling matrix /3 loses rank, and there will no 
longer be a solution to the feedback transformation (17) or (20). The proposed 
methodology assumes that J is non-singular by the class of systems (Section 2.1). 
Practically, one just has to ensure J{x) is non-singular in the neighbourhood of 

r = h-^{v). • 

3.4 Continuity of control input 

The tangential and transversal dynamics using the linearizing control input v 
become a series of double integrators. Stabilizing such dynamics can be done 
using linear control techniques. For the path V it would be nice for the control 
input u to remain continuous as the output moves from one curve to another 
within V. 
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Theorem 3.8. If the auxiliary input v is chosen to he a continuous function of 
Tk{x) with k = k* selected by Algorithm 1, then the control input u is continuous 
for the entire splined path V. 

Proof If is a continuous function of 77 and then v is continuous if and only 
if the r] and f states are continuous. This is in fact the case by Proposition 3.3, 
thus V is continuous over the entire path. 

The control input u is calculated based on the feedback transform (17) for 
the fc’th spline: 

V = I3k*{x)u + ak-{x) 

Since v is continuous, u is continuous if and only if /3fc* (x) and (x) are 
continuous (assuming the optimization to solve for u in the redundant case is 
also smooth). Equivalently, in a similar manner to Proposition 3.3, we must 
show that 

• Pk{x) and ak(x) are continuous given fc; 

• (3k{x) and ak{x) are continuous at the transistion of k: 

(Vx G {x e K”|x = {Wk O h{x))~^{X(^k,max))}) 

I3k{x) = /3fc+i(x) , ak{x) = ak+i(x) 

Using the definition for I3k{x) from Section 3.2, the term J{xc)gv{xc) is 
continuous over the path (by system assumptions) and is not dependent on the 
spline k. The remaining terms are all used in Tk{x), and thus it follows from 
Proposition 3.3 that j3k(x) is continuous. 

Similarly, using the definition for ak{x) from Section 3.2, the terms e"_(A*), 
j G {1,. .. ,p} are continuous for a given spline k because by using the FS formu¬ 
lae from Proposition 3.3, the highest derivative that appears will be o’[^~''^^(A*); 
our path is by Assumption 1. The remaining terms in a are continu¬ 

ous since they are the same ones in Tk{x), which were already shown to be 
continuous in Proposition 3.3. 

□ 


4 Experiment 

4.1 System 

The test platform is the Clearpath Robotic Manipulator (CPM) (see Figure 8) 
which is a four degree of freedom, fully actuated system {N = 4). The shoulder, 
elbow, and wrist links are actuated by DC linear actuators. The dynamics are 
derived in [22] and is of the form (1). The dimension of the output space is 
p = 3 - 2/1 and 7/2 are the end-effector positions projected on the ground plane, 
and 1/3 is the end-effector’s height off the ground. The input to the system is 
the motor voltages. Since N > p, the CPM system is redundant. 
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Figure 8: The Clearpath Manipulator (CPM) with the joint coordinates and 
output space labelled. 


4.2 Path Generation 

Waypoints in the 3-dimensional workspace were specified a-priori, and a stan¬ 
dard spline-fitting process using polynomials [20] was used to fit splines through 
these way points while ensuring fourth-derivative continuity. Thus, quintic poly¬ 
nomial splines are required to fit the waypoints. 

A total of 15 way points are specified. The way points were placed to create 
self-intersections in the path to show the robustness of Algorithm 1. Mathemat¬ 
ically, our path is regular and C^, satisfies Assumptions 1 and 2, and is given 
as the following after spline interpolation 


CTfc : Ifc 




fc = l,2,...,14 


where Ifc = [0, Ik], and Ik is the chord length between way point k and fc -|- 1. 


4.3 Control Design 

The first step in the control design is to define the coordinate transformation. 
The k* and A* are determined by Algorithm 1. The initial guess for Algorithm 
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1 is determined a-priori by solving the optimization globally using brute force 
and given the (known) initial output position j/o = h{xo). Then, when the real 
time control law is run, Algorithm 1 is used to determine the k* and A* at each 
time step. The coordinate transformation follows from Section 3.1.4. Since 
n — 2p = 2, we need a map (pk ■ R" —t to complete the diffeomorphism. We 
let (x) = Xc 2 + 2^03 and {p 2 k (x) = Xy^ +a :„3 +Xy^ for all k to represent the 
angle and its rate, respectively, of the end-effector with respect to the ground 
plane. It can be shown that this completes the diffeomorphism. 

The dynamics in the transformed space are then 


m = m 

f ]2 = (x) + LgLjrji* {x)u 

^2 = + LgLf^l,Jx)u 



= Lj^^.Jx) + LgLf^^.Jx)u 
Cl = (2 

4 

C2 = '^lfv(x) +gy(x)u]i 

i=2 

where x = ^), k* is chosen by Algorithm 1, and C represent the redun¬ 

dant dynamics for path following. 

Setting V = j5k*{x)u ak»{x) as previously defined in (17), the following 
partially linearised system is obtained: 

m = m C! = C2' C? = Cl 

m = Vg cl = Cl = 

C = fc{x,v) 

where x = s-^d f( represents the redundant dynamics based on our 

choice of control input u. The desired task is to track a constant tangential 
velocity profile (i.e., to track some ry™^) while staying on the path (force C to go 
to zero). Designing a controller for i; is a linear control design problem; the one 
found in [22] is used as it was shown to be robust to the modelling inaccuracies. 
The tangential controller is a PI controller 

Vg{r], t) = Kp - 772(0) -f 

r (21) 

Ki J {t] 2^ - mix)) dr. 

0 


22 


For the transversal controller, let ^ = (^i,^ 2 )Ci jC l)) ^5 = (^Ci)'*^C 2 )- Then, 



(22) 


where K,Ko,Ki,K 2 G G K are constants that depend on a robustness 

criteria and are used as tuning parameters [22, 24]. 

To solve for u based on the auxiliary control v, the optimization is done as 
outlined in Section 3.3 using (19). The actuation limits are = 

50% duty cycle for i = 1,..., 4. The joint limit values . were set to 

their true limits for joints i = 1, 2,3, and for the wrist joint (i = 4) two different 
runs were done, one with limits set to 0 — 45 degrees and another to 45 — 90 
degrees. The results are found in the subsequent section. 

We implement the path following controller on the CPM via Labview Real- 
Time Module® to control the motor PWM amplifiers and to read the optical 
encoders. The loop rate of the controller is 20 ms. The encoders read the 
linear actuator distances, so an inverse measurement model is used to retrieve 
the states Xc of the system. The encoder resolution for the waist is l.iE — 5 
rad, and for the linear actuators is A.2E — 5 inch. The derivative of Xc is 
numerically computed to retrieve the Xy states using a first-order approximation. 
The tangential controller (21) is the only dynamic part of the control algorithm, 
and is discretized also using a first-order approximation. 

4.4 Results 

The results of the path following controllers using both joint limit values can be 
found in Figures 9-13. The output of Algorithm 1 is also shown for reference, 
with the number of iterations the gradient descent takes to converge at each 
time step shown. 

The path following controller naturally converges to the closest point on the 
path due to the explicit stabilization of the path following manifold. 

Note that in the Cartesian plot, the responses are very similar, with sim¬ 
ilar path errors. The transformed state plots also show this as expected: the 
controllable ij and ^ subsystems behave the same in both scenarios, because 
these subsystems, under the feedback transform, do not depend on the redun¬ 
dant state dynamics (C). The redundant state C trajectory differs, however, 
since we are using the redundancy to maintain different joint angle limits using 
(18),(19). The plots show well-behaved dynamics and boundedness for ^ as we 
had postulated for systems with inherent damping. 

Closer inspection of the Xc states in Figure 12 shows that indeed our con¬ 
troller was able to satisfy the respective joint angle limits. The angle trajecto¬ 
ries are also slightly different for the first three joints, because maintaining the 
output on the path with a different preferred position of the wrist forces the 
angles for the elbow and shoulder links to adjust accordingly. All this is done 
automatically by (18),(19). 

Figure 13 shows well behaved control action without chattering. 
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* Robust path following control, wrist limits: 0 to j 
Robust path following control, wrist limits: f to | 

-- Spline interpolated reference path 

• Waypoints 



5 Conclusion and Future Work 

This paper proposes a method for following general paths in the form of se¬ 
quences of curves; most notably are spline interpolated paths which can be 
made to go through arbitrary way points. The path following manifold cor¬ 
responding to this path is stabilized, thus rendering the path attractive and 
invariant. A numerical algorithm is used to allow the use of paths which are 
self-intersecting. 

Furthermore, a redundancy resolution scheme is proposed based on a static 
optimization. A conjecture is made stating that this scheme can yield bounded 
zero-dynamics and, in particular, satisfy an objective of staying away from joint 
limits. This scheme has been validated in two analytical examples, as well as on 
our experimental platform. A rigorous proof may be direction for future work. 
Also, a dynamic optimization may be done to account for the system dynamics 
in the case that the system does not have inherent damping, which may also be 
direction for future work. 
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Figure 10: Algorithm 1 information. 


Appendix A Numerical Optimization 

Due to the nature of numerical algorithms, the computation of A* and k* for t > 0 is done 
discretely. At the next time-step t = At (where At is sampling time of the computer) and as 
the output y moves, the next, closest local minimum of \\y — (A)|| is the global minimum, 

assuming that the output y hasn’t moved far within the time step. Thus, simple numerical 
algorithms like gradient descent can be used to find the nearest local minimum which will end 
up being the global minimum. The idea is illustrated in Figure 14 below. 

For a moment assume that the output y is already on the path {y = (^*))- If the output 

y moves too far within one time step (corresponds to a large change in the path parameter 
A*), then there may be a local minimum between the initial guess (the previous time-step’s 
solution) and the true global minimum. Thus, we can ensure that a numerical algorithm will 
not get stuck at such a well by ensuring the path parameter only changes by some amount AA, 
which keeps the function \\y — ('^)||y—( a*) convex for A G [A* — AA, A* -h AA]. If 

is unit-speed parametrized, i.e., ||(t'(A)|| = 1 for all A, then AA corresponds to the allowable 
distance along the curve that can be travelled. 
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This AA is a conservative estimate for the allowable change in parameter in one time-step, 
since requiring the function to be convex is sufficient, not necessary to ensure convergence of 
a numerical algorithm to A* if initialized properly. In logic notation, AA must satisfy 


(VA* e Ifc-) (VA e [A* - AA, A* -h AA] n Ifc.) 


which is equivalent to 


dA2 


> 0 


(VA* e ifeO (VA e [A* 


AA, A* -h AA] n Ifc.) 

(o-fe* (A*) - (Tj,. (A), (A)) -h 

(A*) - o-fc. (A), (A))^ 

lkfc*(A*) - o-j,.(A)]]^ 


-wir 


(A.l) 


The relation in (A.l) can be used as a test to infer the allowable speed at which the 
output y can traverse a path in order for a numerical optimizer to succeed in evaluating 
'ujk*{y) = arginf_;^^][^^ Wv ~ particular for a unit-speed parametrized curve and 

assuming the output is on the path V, if (A.l) holds for some AA, then the allowable speed 
along the curve (772) is 
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Figure 12: Joint angles. 


Example 3. (Ellipse) Let ngpime = 1 and consider the path of a single ellipse (the /c* 
subscript will be dropped) (t(A) = (2 cos(A), sin(A)). When A* = 0, all A G [—1.5136,1.5136] 
satisfies the inequality in (A.l). This means that when the output is at cr(0), the change in 
the path parameter in one time step is +/ — 1.5136. This value can be solved for analytically 
or numerically using the inequality in (A.l). We can run the same test for all A* over the 
path and take the smallest resulting range to be the AA. A 

A numerical algorithm like steepest descent works for finding A* if it starts within AA of 
A*, however it normally takes many iterations to converge since the algorithm takes small step 
sizes to the solution [25]. Another approach is to use some knowledge about to adjust 
the step size. In particular, if the path is unit-speed parametrized, one could use an initial 
step size of /S,t in the direction of the steepest descent to quickly approach the solution. If 
the path is not unit-speed parametrized, a map of and A* can be constructed using (7) 

sampled at each time step, and the smallest jump in A* can be used as the initial step size. 
An algorithm like monotonic gradient descent with adaptive step-size works nicely (Algorithm 
1 ) using this initial step size [26]. 

Determining k* is just a matter of checking if the A* computed by the numeric algorithm 
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Figure 13: Control effort. 


is outside the domain Ifc*. If it is, then k* must be incremented or decremented, as done in 
Algorithm 1. Let := Ijt/— (A)|| be the function to be minimized. The algorithm 

for determining A* and k* can be found in Algorithm 1. 

Algorithm 1 is ran at each time step in order to calculate the coordinate transformation. 
In Algorithm 1, lines 1 to 11 are the familiar monotonic gradient descent algorithm with 
stepsize adaptation [26]. Lines 12 to 18 determine which spline we are on. Depending on the 
path following application, if there is a known neighbourhood of the tangential velocity, the 
stepsize a in Algorithm 1 can be tuned in order to minimize the number of steps taken in the 
gradient descent and thus the computational load. 


Appendix B Supporting proofs and Derivations 

Proof of Lemma 3.2 . Note that (6) is solved for by Algorithm 1, so if the output y is equidis¬ 
tant to multiple points on the path, then a A* is always well defined by the local search. 
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Figure 14: At the start of the run, the true A* is known. As the output y moves, at 
each subsequent time-step a numerical optimizer is likely to find the global minimum of 
\\y — Ofi* (A)|| if initialized at the previous time-step’s A*, since the global minimum will be 
near. 


Next we write the differentials for each function. 


drji 

dx 




dm 

dx 

di{-^ 

dx 


d^ 

dx 


[ei(A*)T J(a;c) , Oixjv] 

[* , ei(A*)^ J(a;c)] 

({h{x) - .(A*))^ (A*)er(A*)T) 

+ (e,(A*)T)) J(a:,) , Oixjv] 


where * denotes a vector of dimension 1 x A^. Looking at the transversal terms, the product 
{h{x) — f7(A*))^ produces a scalar, call it a. Thus the terms become 


dx 

dx 


[(aei(A*)^ + ej(A*)^) J(a;c) , OixAt] 

[* , (aei(A*)T+e^(A*)T) J(a;,)] 
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Algorithm 1 Determining A* and /c* at each time step. 

Input: The closest spline k* and corresponding A*, stepsize a, the gradient and 

an e for the stopping criterion. 

Output: The closest spline k* and the A* minimizing t^fc*(A) = \\y — f7fc*(A)||. 

1: Initialize 9= 

2: repeat 

3: X*' -ag/\\g\\, (A*) 

4: if '^x*^ < ^A* then 

5i A* i — A*^, ^A* ^— tvx*' 

6 : 

7: a 1.2a {increase the stepsize} 

8: else 

9: a 0.5a {decrease the stepsize} 

10: end if 

11: until |A*' - A*| < e 

12: if A* > max(Ifc*) then 

13: fc* /c* + 1 

14: GOTO Line 1 

15: else if A* < min(Ifc*) then 

16: fc* fc* — 1 

17: GOTO Line 1 

18: end if 


Since the domain U does not include singularity points of J{xc) by the class of systems, the 
differentials will be linearly independent if the vectors ei,i = 1, ...,p are orthogonal. These 
vectors are orthogonal because they are the FS orthonormal basis vectors. □ 


Derivation of (8). By the fundamental theorem of calculus ('^*)||* Geometric 

arguments provide that = Ki^*{y)a'f^^(X*)~^ where /Cfc* : MP —>■ R is a smooth scalar 

function [27]. To solve for Kk*{y)i the following identity may be used: 


{y) = ^k* i^k* i'^k* (y)))- 

Differentiating both sides: 

dizujg* I 


'W|a=. 


dizu}^* 


lA—A* 
= 1 


dy dy 

dm^* , I 

0'fe»('^)L=A* = {^k-{yWk*i>^*P) <*(^*) = 1 

^Kk-{y) = 


Thus 772 can be compactly written as (8). 


□ 


Derivation of (12). 


First note the identity ^ej(A*) = ej{X*)^-y, and = 
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from above. Then, 


er:=,r = (e'(V)- 


dzu 


y = h{x) 


J{xc)xv {h{x) — cr(A*)) 


dzj 


+ ej(A*)' [ J{xc)xv - cr'{X*) — 

dy 




y=h{x) 


J{Xc)Xi 


= ( 77-777::^) (^(*) - ^(^*)) 


||f7'(A* 


+ ej(A*)^ (^J{xc)xv — ei(A*)ei(A*)^J(xc)a;„^ 


and using the simplification that (ej,ei} = 0 for j > 1 and ri 2 {x) = {gi, J{xc)xv) yields 
( 12 ). □ 

Generalized FS equations [28]. 


e'i(A) 

e'2(A) 


';-i(A) 

- e(,(A) _ 


= lk'W|| 


■ 0 

XliX) .. 

0 

0 


■ ei(A) 

-XiW 

0 

0 

0 


e2(A) 

0 

0 

0 

A’p-i(A) 


ep-i(A) 

0 

0 

1 

1 

0 


- ep(A) _ 


where A’i(A) = ^ !}• 


(A.2) 


□ 
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